import { _decorator, Vec3 } from 'cc';
import { Astar } from './Astar';
import { Channel } from './Channel';
import { nUtils } from './nUtils';
import { Plane } from './Plane';
import { Triangle } from './Triangle';
const closestPoint = new Vec3
const plane = new Plane
const triangle = new Triangle
const point = new Vec3
const endPoint = new Vec3
const Infinity = Math.pow(50, 2)


export class NavMesh {
    public static zone = {}

    /**
     * @Date: 2022-02-26 00:54:29
     * @LastEditors: iwae
     * @description: 
     * @param {*} zoneID
     * @param {*} data
     */
    static setZoneData(zoneID: string, data) {
        this.zone[zoneID] = data;
    }

    static clearZoneData(zoneID: string) {
        this.zone[zoneID] = null;
    }
    /**
     * @Date: 2022-02-26 00:53:42
     * @LastEditors: iwae
     * @description: 获取Navmesh Agent的组Id
     * @param {string} zoneID 区域ID
     * @param {Vec3} position Navmesh Agent的当前坐标
     */
    static getGroup(zoneID: string, position: Vec3): number {
        if (!this.zone[zoneID]) return null;
        let closestNodeGroup = null;
        let distance = Infinity;
        for (let i = 0, len = this.zone[zoneID].groups.length; i < len; i++) {
            const group = this.zone[zoneID].groups[i];
            for (let j = 0, len2 = group.length; j < len2; j++) {
                const node = group[j];
                let measuredDistance = Vec3.squaredDistance(node.centroid, position);
                if (measuredDistance < distance) {
                    closestNodeGroup = i;
                    distance = measuredDistance;
                }
            }
        }
        return closestNodeGroup;
    }
    /**
     * @Date: 2022-02-1 10:30:10
     * @LastEditors: iwae
     * @description: Returns a random node within a given range of a given position.
     * @param {string} zoneID
     * @param {number} groupID
     * @param {Vec3} nearPosition
     * @param {number} nearRange
     */
    static getRandomNode(zoneID: string, groupID: number, nearPosition: Vec3, nearRange: number): Vec3 {
        if (!this.zone[zoneID]) return new Vec3();
        nearPosition = nearPosition || null;
        nearRange = nearRange || 0;
        const candidates = [];
        const polygons = this.zone[zoneID].groups[groupID];
        polygons.forEach((p) => {
            if (nearPosition && nearRange) {
                if (Vec3.squaredDistance(nearPosition, p.centroid) < nearRange * nearRange) {
                    candidates.push(p.centroid);
                }
            }
            else {
                candidates.push(p.centroid);
            }
        });
        return nUtils.sample(candidates) || new Vec3();
    }

    /**
     * @Date: 2022-02-26 00:52:34
     * @LastEditors: iwae
     * @description: Navmesh导航核心方法
     * @param {Vec3} startPosition 起始坐标
     * @param {Vec3} targetPosition 目标坐标
     * @param {string} zoneID 区域Id
     * @param {number} groupID 分组ID
     */
    static findPath(startPosition: Vec3, targetPosition: Vec3, zoneID: string, groupID: number) {
        const allNodes = this.zone[zoneID].groups[groupID];
        if (!allNodes) return null;
        const vertices = this.zone[zoneID].vertices;
        let closestNode = null;
        let distance = Infinity;
        let measuredDistance
        for (let i = 0, len = allNodes.length; i < len; i++) {
            const node = allNodes[i];
            measuredDistance = Vec3.squaredDistance(node.centroid, startPosition);
            if (measuredDistance < distance) {
                closestNode = node;
                distance = measuredDistance;
            }
        }
        let farthestNode = null;
        distance = Infinity;
        for (let i = 0, len = allNodes.length; i < len; i++) {
            const node = allNodes[i];
            measuredDistance = Vec3.squaredDistance(node.centroid, targetPosition);
            if (measuredDistance < distance &&
                nUtils.isVectorInPolygon(targetPosition, node, vertices)) {
                farthestNode = node;
                distance = measuredDistance;
            }
        }
        // If we can't find any node, just go straight to the target
        if (!closestNode || !farthestNode) {
            return null;
        }
        //A*寻路算法
        const paths = Astar.search(allNodes, closestNode, farthestNode);
        const getPortalFromTo = function (a, b) {
            for (let i = 0; i < a.neighbours.length; i++) {
                if (a.neighbours[i] === b.id) {
                    return a.portals[i];
                }
            }
        };
        //使用漏斗算法，结算出最佳路线
        // Got the corridor,pull the rope
        const channel = new Channel();
        channel.push(startPosition);
        for (let i = 0; i < paths.length; i++) {
            const polygon = paths[i];
            const nextPolygon = paths[i + 1];
            if (nextPolygon) {
                const portals = getPortalFromTo(polygon, nextPolygon);
                channel.push(vertices[portals[0]], vertices[portals[1]]);
            }
        }
        channel.push(targetPosition);
        channel.stringPull();
        // Return the path, omitting first position (which is already known).
        return channel.path;
    }

    /**
     * 返回离目标点附近最近的可以移动点.
     * @param  {Vec3} position
     * @param  {string}  zonesID
     * @param  {number}  groupID
     * @param  {boolean} checkPolygon
     * @return {Node}
     */
    static getClosestNode(position: Vec3, zonesID: string, groupID: number, checkPolygon = false): Vec3 {
        const nodes = this.zone[zonesID].groups[groupID];
        const vertices = this.zone[zonesID].vertices;
        let closestNode = null;
        let closestDistance = Infinity;

        nodes.forEach((node) => {
            const distance = Vec3.squaredDistance(node.centroid, position);
            if (distance < closestDistance
                && (!checkPolygon || nUtils.isVectorInPolygon(position, node, vertices))) {
                closestNode = node;
                closestDistance = distance;
            }
        });

        return closestNode;
    }


    /**
     * Clamps a step along the navmesh, given start and desired endpoint. May be
     * 常用于手柄或者WASD控制
     *
     * @param  {Vec3} endRef Desired endpoint.
     * @param  {Node} node
     * @param  {string} zonesID
     * @param  {number} groupID
     * @param  {Vec3} endTarget Updated endpoint.
     */
    static clampStep(endRef: Vec3, node, zonesID, groupID, endTarget: Vec3) {
        let closestDistance;
        let distance;
        const vertices = this.zone[zonesID].vertices;
        const nodes = this.zone[zonesID].groups[groupID];
        const nodeQueue = [node];
        const nodeDepth = {};
        nodeDepth[node.id] = 0;
        closestPoint.set(0, 0, 0);
        closestDistance = Infinity;
        // Project the step along the current node.
        plane.setFromCoplanarPoints(
            vertices[node.vertexIds[0]],
            vertices[node.vertexIds[1]],
            vertices[node.vertexIds[2]]
        );
        plane.projectPoint(endRef, point);
        endPoint.set(point)
        for (let currentNode = nodeQueue.pop(); currentNode; currentNode = nodeQueue.pop()) {
            triangle.set(
                vertices[currentNode.vertexIds[0]],
                vertices[currentNode.vertexIds[1]],
                vertices[currentNode.vertexIds[2]]
            );

            triangle.closestPointToPoint(endPoint, point);
            distance = Vec3.squaredDistance(point, endPoint)
            if (distance < closestDistance) {
                closestPoint.set(point);
                closestDistance = distance;
            }
            const depth = nodeDepth[currentNode.id];
            if (depth > 2) continue;
            for (let i = 0; i < currentNode.neighbours.length; i++) {
                const neighbour = nodes[currentNode.neighbours[i]];
                if (neighbour.id in nodeDepth) continue;
                nodeQueue.push(neighbour);
                nodeDepth[neighbour.id] = depth + 1;
            }
        }
        endTarget.set(closestPoint)
    }
};

